#include "Calibration_NinePoint.h"

double Calibration_NinePoint::calibration()
{
	HalconCpp::VectorToHomMat2d(rows_py, cols_px, cols_Rx, rows_Ry, &trans_);

	// 计算转换误差
	HTuple qx, qy;
	HalconCpp::AffineTransPoint2d(trans_, rows_py, cols_px, &qx, &qy);
	HTuple dist;
	HalconCpp::DistancePp(rows_Ry, cols_Rx, qy, qx, &dist);
	HTuple d;
	HalconCpp::TupleMult(dist, dist, &d);
	HTuple sum;
	HalconCpp::TupleSum(d, &sum);
	HTuple rms = 0;
	HalconCpp::TupleSqrt(sum, &rms);

	return rms.D();
}

void Calibration_NinePoint::clearDatas()
{
	cols_px.Clear();
	rows_py.Clear();
	cols_Rx.Clear();
	rows_Ry.Clear();
}

cv::Point2d Calibration_NinePoint::affineTransToRobot(double px, double py)
{
	HalconCpp::AffineTransPoint2d(trans_, px, py, &robot_col, &robot_row);
	image_pos.x = px;
	image_pos.y = py;
	robot_pos.x = robot_col.D();
	robot_pos.y = robot_row.D();

	return robot_pos;
}

bool Calibration_NinePoint::affineTransToRobot(double px, double py, double* rx, double* ry)
{
	HalconCpp::AffineTransPoint2d(trans_, px, py, &robot_col, &robot_row);
	*rx = robot_col.D();
	*ry = robot_row.D();

	image_pos.x = px;
	image_pos.y = py;
	robot_pos.x = robot_col.D();
	robot_pos.y = robot_row.D();

	return true;
}

void Calibration_NinePoint::addPoint_pixel(double x, double y)
{
	cols_px.Append(x);
	rows_py.Append(y);
}

void Calibration_NinePoint::addPoint_robot(double x, double y)
{
	cols_Rx.Append(x);
	rows_Ry.Append(y);
}

void Calibration_NinePoint::writeTransFile(std::string fileName)
{
	HTuple serialHandle;
	HalconCpp::SerializeHomMat2d(trans_, &serialHandle);
	HTuple fileHandle;
	HalconCpp::OpenFile(fileName.c_str(), "output_binary", &fileHandle);
	HalconCpp::FwriteSerializedItem(fileHandle, serialHandle);
	HalconCpp::CloseFile(fileHandle);
}

void Calibration_NinePoint::readTransFile(std::string fileName)
{
	HTuple fileHandle;
	HalconCpp::OpenFile(fileName.c_str(), "input_binary", &fileHandle);
	HTuple serialHandle;
	HalconCpp::FreadSerializedItem(fileHandle, &serialHandle);
	HalconCpp::DeserializeHomMat2d(serialHandle, &trans_);
	HalconCpp::CloseFile(fileHandle);
}
